import math
import numpy as np

pren=20
PI=math.pi
#别忘了把i重置

class LQR_controller:
    def __init__(self):
        self.lf=0.65
        self.lr=0.55
        self.m=120
        self.L=1.2
        self.Cf=-20000
        self.Cr=-20000
        self.i = 0  # 运行到的点位
        self.x = 0
        self.y = 0
        self.head = 0
        self.v = 10
        self.ki = 0
        self.kd = 0
        self.kp = 0.15
        self.steer = 0
        self.thr = 0
        self.brake = 0
        self.K = np.zeros((3000, 4))
        self.y_bias_past = 0
        self.h_bias_past = 0
        self.I_jilu=np.zeros(10)
        self.I_jilu=list(self.I_jilu)
        self.pre_e=0
        self.tar_xlist=None
        self.tar_ylist = None
        self.tar_hlist = None
        self.tar_vlist = None
        self.tar_klist = None
        self.lqr_mode=0    #1是开始寻迹，0是结束寻迹

    def update_ego_state(self,curx,cury,curhead,curv):
        self.x=curx
        self.y=cury
        self.head = curhead
        self.v=curv

    def update_trajectory(self, xlist, ylist, hlist, vlist):
        self.LQR_Init()
        self.tar_xlist,self.tar_ylist,self.tar_hlist,self.tar_vlist=self.preprocess_data(xlist,ylist,hlist,vlist)
        #算曲率要在数据预处理之后，要不会出现非常奇怪的曲率
        self.tar_klist=self.cal_k(self.tar_xlist,self.tar_ylist)
        for i in range(0,len(self.tar_vlist)):
            if self.tar_vlist[i]<0.1:
                self.tar_vlist[i]=0.1


    def preprocess_data(self,x,y,h,v):
        xtemp = [x[0]]
        ytemp = [y[0]]
        htemp = [h[0]]
        vtemp = [v[0]]
        sum_d=0
        for i in range(1,len(x)):  # 生成一系列有规律间隔的点
            sum_d+=math.hypot(x[i] - x[i-1], y[i] - y[i-1])
            if sum_d > 0.1:
                xtemp.append(x[i])
                ytemp.append(y[i])
                htemp.append(h[i])
                vtemp.append(v[i])
                sum_d=0
        return xtemp,ytemp,htemp,vtemp

    def closepoint(self):#pren为前瞻步数
        if self.i+pren>=len(self.tar_xlist):
            n=len(self.tar_xlist)-self.i
        else:
            n=pren
        bx=self.tar_xlist[self.i:self.i+n]
        by=self.tar_ylist[self.i:self.i+n]
        x0list=np.ones(len(bx))*self.x
        y0list=np.ones(len(by))*self.y
        dMat=np.square(bx-x0list)+np.square(by-y0list)
        self.i=np.where(dMat==np.min(dMat))[0][0]+self.i


    def clockwise_0_360_to_pi_negative_pi(self,angle):
        angle=-angle
        angle=angle/180*math.pi
        while angle>=math.pi:
            angle-=2*math.pi
        while angle < - math.pi:
            angle += 2 * math.pi
        return angle

    def NormalizeAngle(self,angle):
        while angle >= math.pi:
            angle -= 2 * math.pi
        while angle < - math.pi:
            angle += 2 * math.pi
        return angle


    def track(self):
        self.closepoint()
        if (self.i < len(self.tar_xlist)-1) and self.lqr_mode==1:
            match_x = self.tar_xlist[self.i]
            match_y = self.tar_ylist[self.i]
            match_h = self.clockwise_0_360_to_pi_negative_pi(self.tar_hlist[self.i])
            match_v = self.tar_vlist[self.i]

            head_hudu= self.clockwise_0_360_to_pi_negative_pi(self.head)
            # 计算横向偏差，角度偏差，横向变化率，角度变化率。投影点heading近似认为是匹配点heading
            # 计算横向偏差
            match_tao = np.array([[-math.sin(match_h)], [math.cos(match_h)]])#math库中的三角函数运算符输入是弧度值
            d_x_matchx = np.array([self.x - match_x, self.y - match_y])
            l_bias = np.dot(d_x_matchx, match_tao)  # 车左偏为正，右偏为负
            l_bias = l_bias[0]
            if l_bias>=9999 or l_bias<=-9999:
                self.lqr_mode=0
            #计算heading偏差
            # if (head_hudu-match_h)>math.pi:
            #     h_bias=(head_hudu-match_h)-2*math.pi
            # elif (head_hudu-match_h)<-math.pi:
            #     h_bias = (head_hudu - match_h) + 2 * math.pi
            # else:
            #     h_bias = head_hudu - match_h
            h_bias=self.NormalizeAngle(head_hudu-match_h)
            #计算横向偏差变化率
            d_l_bias = self.v * (math.sin(h_bias))
            #计算heading偏差变化率
            d_h_bias = (h_bias - self.h_bias_past) / 0.1  # 这个0.1是track执行的周期，我还不知道具体是多少
            self.h_bias_past = h_bias  # 用作下一帧求d_h_bias
            #召唤lqr控制函数
            psteer = 180 / PI * self.LQR_Control(l_bias, d_l_bias, h_bias, d_h_bias)
            psteer = - psteer[0, ]
            if psteer > 15:  # 转向限制
                psteer = 15
            else:
                if psteer < -15:
                    psteer = -15

            v_error=self.v-match_v
            a_zhengze=self.PID_control(v_error)
            if a_zhengze>=0.5:
                a_zhengze=0.5
            if a_zhengze<=-1:
                a_zhengze=-1
            if a_zhengze>=0:
                thr=a_zhengze
                brake=0
            else:
                thr=0
                brake=-a_zhengze
            self.steer = float(psteer)/15
            self.thr =thr
            self.brake =brake
        else:
            self.lqr_mode=0
            self.steer = 0
            self.thr = 0
            self.brake = 0.95

        #停住之后初始化
        if self.lqr_mode==0:
            self.steer=0
            self.thr=0
            self.brake=0.8
            self.i=0
            self.I_jilu = np.zeros(10)
            self.I_jilu = list(self.I_jilu)
            self.y_bias_past = 0
            self.h_bias_past = 0
            self.pre_e = 0

        return self.lqr_mode

    def LQR_Init(self):
        self.K = np.loadtxt('K.txt')

    def LQR_Control(self, l_bias, d_l_bias, h_bias, d_h_bias):
        X = np.array([[l_bias], [d_l_bias], [h_bias], [d_h_bias]])
        xvhao=(int)(self.v*100)
        if(xvhao>=3000):
            k=self.K[2999,:]
        else:
            k = self.K[xvhao, :]
        u_fankui= -np.dot(k, X)
        u_qiankui=(self.m*self.v*self.v*self.tar_klist[self.i]/self.L)*(self.lr/(2*self.Cf)-self.lf/(2*self.Cr)
                +self.lf*k[2]/(2*self.Cr))\
                +self.L*self.tar_klist[self.i]\
                -self.lr*self.tar_klist[self.i]*k[2]
        print("u反馈=", u_fankui)
        print("u前馈=", u_qiankui)
        u=u_fankui+u_qiankui
        return u

    def PID_control(self, e):
        e=e
        self.I_jilu.pop(0)
        self.I_jilu.append(e)

        d_temp=(e-self.pre_e)/0.1
        self.pre_e=e

        thr_P=self.kp*e
        thr_I=self.ki*sum(self.I_jilu)
        thr_D=self.kd*d_temp

        u=thr_P+thr_I+thr_D
        u=-u
        return u

    def cal_k(self,x, y):
        k = [0]
        for i in range(1, len(x) - 1):
            a_index = i - 1
            b_index = i
            c_index = i + 1
            d_a = math.sqrt(pow(x[b_index] - x[c_index], 2) + pow(y[b_index] - y[c_index], 2))
            d_b = math.sqrt(pow(x[a_index] - x[c_index], 2) + pow(y[a_index] - y[c_index], 2))
            d_c = math.sqrt(pow(x[b_index] - x[a_index], 2) + pow(y[b_index] - y[a_index], 2))
            if d_a < 0.01:
                d_a = 0.01
            if d_c < 0.01:
                d_c = 0.01
            cos = (d_a * d_a + d_c * d_c - d_b * d_b) / (2 * d_a * d_c)
            # 这里可能是精度上有差异，竟然能算出大于1的cos
            if ((1 - cos * cos) < 0):
                k_temp = 0.01
            else:
                sin = math.sqrt(1 - cos * cos)
                k_temp = 2 * sin / d_b
            k.append(k_temp)
        k.append(0)
        return k



